#include "ClothScene.h"

#include <NxCooking.h>
#include <NxTriangleMeshDesc.h>
#include <NxPlaneShapeDesc.h>
#include <cloth/NxClothDesc.h>
#include <cloth/NxClothMesh.h>
#include <cloth/NxClothMeshDesc.h>

ClothScene::ClothScene()
{
    testcloth = NULL;
    testSoftBody = NULL;
}

ClothScene::~ClothScene()
{
    CleanScene();
}

void ClothScene::PrepareScene(NxScene *scene)
{
    PhysxLogic::cam = new Camera(NxVec3(15.0f, 10.0f, 15.0f), NxVec3(-0.7f,-0.5f,-0.7f), (float)glutGet(GLUT_WINDOW_WIDTH)/(float)glutGet(GLUT_WINDOW_HEIGHT), 60.0f, 0.1f, 10000.0f);

    //default material
    NxMaterial *defaultMaterial = scene->getMaterialFromIndex(0);
    defaultMaterial->setRestitution(0.5f);
    defaultMaterial->setStaticFriction(0.5f);
    defaultMaterial->setDynamicFriction(0.5f);

    //create plane
    NxPlaneShapeDesc planeDesc;
    NxActorDesc actorDesc;
    actorDesc.shapes.pushBack(&planeDesc);
    scene->createActor(actorDesc);

    //Create cubes
    //PhysxLogic::CreateBox(NxVec3(0.0f, 4.0f, 0.0f), NxVec3(0.5f, 0.5f, 0.5f), NxVec3(0.0f, 0.0f, 0.0f), false, 1.0f);
    //CreateBox(NxVec3(0.0f, 5.0f, 0.0f), NxVec3(0.05f, 0.05f, 0.05f), NxVec3(0.0f, 0.0f, 0.0f), false, 1.0f);
    //CreateBox(NxVec3(0.0f, 6.0f, 0.0f), NxVec3(0.05f, 0.05f, 0.05f), NxVec3(0.0f, 0.0f, 0.0f), false, 1.0f);
    //testcloth = CreateCloth(NxVec3(0,10,0), 10, 10, 0.5f, scene);

    testSoftBody = CreateSoftBodyBox(NxVec3(15,5,0), NxVec3(10,10,10), scene);
    //PhysxLogic::CreateBox(NxVec3(0,10,0), NxVec3(1,1,1)*1)->userData = (void*)this;
    //testSoftBody->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);

    /*PhysxLogic::CreateBox(NxVec3(0,15,0), NxVec3(1,1,1)*0.8)->userData = (void*)this;
    CreateSoftBodyBox(NxVec3(0,15,0), NxVec3(2,2,2), scene)->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);

    PhysxLogic::CreateBox(NxVec3(0,20,0), NxVec3(1,1,1)*0.8)->userData = (void*)this;
    CreateSoftBodyBox(NxVec3(0,20,0), NxVec3(2,2,2), scene)->attachToCollidingShapes(NX_SOFTBODY_ATTACHMENT_TWOWAY);*/

   /* Model m;
    m.Load("..\\res\\icoSphere.obj", 1.0, &scene->getPhysicsSDK(), ModelBoundingVolume::TRIANGLE_MESH_SHAPE);
    NxTriangleMeshShapeDesc *modelShape = m.GetModelparts()[0]->GetBoundingVolume()->IsTriangleMeshShape();
    testcloth = CreateClothFromMesh(NxVec3(0,5,0), modelShape, scene);*/
    //CreateCloth(NxVec3(0,10,0), 10, 10, 3, scene);

    testcloth = CreateClothBox(NxVec3(0,5,0), NxVec3(10,10,10), scene);
    //NxActor *boxActor = PhysxLogic::CreateBox(NxVec3(0,5,0), NxVec3(5,5,5));
    //boxActor->userData = this;
    //cl->attachToCore(boxActor, 600.0f, 0.5f, 1.0f);

    //PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_WORLD_AXES);
    //PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_JOINT_LOCAL_AXES);
    //PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_JOINT_LIMITS);
    //PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_COLLISION_SHAPES);
    //PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_COLLISION_AXES);
    //PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_FORCE_FIELDS);
    //PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_COLLISION_SHAPES);
    PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_COLLISION_SHAPES);
    PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_CLOTH_MESH);	
    PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_SOFTBODY_MESH);	
}

void ClothScene::CleanScene()
{

}

NxCloth *ClothScene::CreateCloth(const NxVec3 &pos, int numX, int numY, float distance, NxScene *scene)
{
    int sizeX = numX + 1;
    int sizeY = numY + 1;

    NxClothMeshDesc clothMeshDesc;
    clothMeshDesc.numVertices				= sizeX * sizeY;
    clothMeshDesc.numTriangles				= numX * numY * 2;
    clothMeshDesc.pointStrideBytes			= sizeof(NxVec3);
    clothMeshDesc.triangleStrideBytes		= 3*sizeof(NxU32);
    clothMeshDesc.vertexMassStrideBytes		= sizeof(NxReal);
    clothMeshDesc.vertexFlagStrideBytes		= sizeof(NxU32);
    clothMeshDesc.points					= new NxVec3[clothMeshDesc.numVertices];
    clothMeshDesc.triangles					= new NxU32[clothMeshDesc.numTriangles*3];
    clothMeshDesc.vertexMasses				= 0;
    clothMeshDesc.vertexFlags				= 0;
    clothMeshDesc.flags						= 0;

    float offsetX = numX * 0.5f * distance;
    float offsetY = numY * 0.5f * distance;
    NxVec3 *p = (NxVec3*)clothMeshDesc.points;
    int vCount = 0;
    for (int i = 0; i <= numY; i++) {
        for (int j = 0; j <= numX; j++) {
            p[vCount].set(distance*j-offsetX, 0.0f, distance*i-offsetY); 
            vCount++;
        }
    }

    int e0,e1,e2,e3; //edgesIndices
    int iCount = 0;
    NxU32 *id = (NxU32*)clothMeshDesc.triangles;
    for (int i = 0; i < numY; i++) {
        for (int j = 0; j < numX; j++) {
            e0 = i * sizeX + j;
            e1 = e0 + 1;
            e2 = e0 + sizeX;
            e3 = e2 + 1;
            if ((j+i)%2) {
                id[iCount++] = e0; id[iCount++] = e2; id[iCount++] = e1;
                id[iCount++] = e1; id[iCount++] = e2; id[iCount++] = e3;
            }
            else {
                id[iCount++] = e0; id[iCount++] = e2; id[iCount++] = e3;
                id[iCount++] = e0; id[iCount++] = e3; id[iCount++] = e1;
            }
        }
    }

    //
    NxCookingInterface *cooking = NxGetCookingLib(NX_PHYSICS_SDK_VERSION);

    cooking->NxInitCooking();

    MemoryWriteBuffer wb;
    cooking->NxCookClothMesh(clothMeshDesc, wb);
    MemoryReadBuffer rb(wb.data);
    NxClothMesh* clothMesh = scene->getPhysicsSDK().createClothMesh(rb);
    //

    const NxU32 TEAR_MEMORY_FACTOR = 1;
    NxU32 maxVertices = TEAR_MEMORY_FACTOR * clothMeshDesc.numVertices;
    NxU32 maxIndices = 3 * clothMeshDesc.numTriangles;

    NxClothDesc clothdesc;
    clothdesc.globalPose.t = pos;
    clothdesc.thickness = 0.2f;
    clothdesc.clothMesh = clothMesh;
    clothdesc.meshData = *CreateReceiveBuffer(maxVertices, maxIndices);
    clothdesc.flags |= NX_CLF_VISUALIZATION;
    if(PhysxLogic::IsHardwareAccelerated() == true)
        clothdesc.flags |= NX_CLF_HARDWARE;

    return scene->createCloth(clothdesc);
}

NxCloth *ClothScene::CreateClothBox(const NxVec3 &pos, const NxVec3 &size, NxScene *scene)
{
    int numX = size.x;
    int numY = size.y;
    int numZ = size.z;
    float h = 1.0f;
    int sizeX = numX + 1;
    int sizeY = numY + 1;
    int sizeZ = numZ + 1;

    NxClothMeshDesc clothMeshDesc;
    clothMeshDesc.numVertices = (sizeX * sizeY * sizeZ) - ((numX-1) * (numY-1) * (numZ-1));
    clothMeshDesc.numTriangles = (2*(2 * numX * numY) + 2*(2 * numX * numZ) + 2*(2 * numY * numZ));
    clothMeshDesc.pointStrideBytes = sizeof(NxVec3);
    clothMeshDesc.triangleStrideBytes = 3*sizeof(NxU32);
    clothMeshDesc.vertexMassStrideBytes = sizeof(NxReal);
    clothMeshDesc.vertexFlagStrideBytes = sizeof(NxU32);
    clothMeshDesc.points = new NxVec3[clothMeshDesc.numVertices];
    clothMeshDesc.triangles = new NxU32[clothMeshDesc.numTriangles * 3];
    clothMeshDesc.vertexMasses = 0;
    clothMeshDesc.vertexFlags = 0;
    clothMeshDesc.flags = 0;

    NxU32 *da = new NxU32[sizeX * sizeY * sizeZ];

    NxVec3 offset(h * numX * 0.5f, h * numY * 0.5f, h * numZ * 0.5f);
    NxVec3 *p = (NxVec3*)clothMeshDesc.points;
    int vCount = 0;
    for (int i = 0; i <= numX; i++) {
        for(int j = 0; j <= numY; j++) {
            for(int k = 0; k <= numZ; k++) {
                if(!((h*i < 2*offset.x && h*i > 0) && (h*j < 2*offset.y && h*j > 0) && (h*k < 2*offset.z && h*k > 0))) //no points inside the box
                {
                    p[vCount].set(h*i, h*j, h*k);
                    p[vCount] -= offset;
                    da[GetFlatIndex(sizeY,sizeZ, i,j,k)] = vCount;
                    vCount++;
                }
                else
                {
                    da[GetFlatIndex(sizeY,sizeZ, i,j,k)] = -1;
                }
            }
        }
    }

    int e0,e1,e2,e3; //edgesIndices
    int iCount = 0;
    NxU32 *id = (NxU32*)clothMeshDesc.triangles;

    //left side & right side
    for (int y = 0; y < numY; y++) {
        for (int z = 0; z < numZ; z++) {
            //left side
            da[GetFlatIndex(sizeY,sizeZ,0,y,z)];
            e0 = da[GetFlatIndex(sizeY,sizeZ,0,y,z)];
            e1 = e0 + 1;
            e2 = da[GetFlatIndex(sizeY,sizeZ,0,y+1,z)];
            e3 = e2 + 1;

            id[iCount++] = e1; id[iCount++] = e0; id[iCount++] = e2;
            id[iCount++] = e1; id[iCount++] = e2; id[iCount++] = e3;

            //right side
            e0 = da[GetFlatIndex(sizeY,sizeZ,numX,y,z)];
            e1 = e0 + 1;
            e2 = da[GetFlatIndex(sizeY,sizeZ,numX,y+1,z)];
            e3 = e2 + 1;

            id[iCount++] = e1; id[iCount++] = e0; id[iCount++] = e2;
            id[iCount++] = e1; id[iCount++] = e2; id[iCount++] = e3;
        }
    }

    //back side & front side
    for (int y = 0; y < numY; y++) {
        for (int x = 0; x < numX; x++) {
            //back side
            e0 = da[GetFlatIndex(sizeY,sizeZ,x,y,0)];
            e1 = da[GetFlatIndex(sizeY,sizeZ,x+1,y,0)];
            e2 = da[GetFlatIndex(sizeY,sizeZ,x,y+1,0)];
            e3 = da[GetFlatIndex(sizeY,sizeZ,x+1,y+1,0)];

            id[iCount++] = e1; id[iCount++] = e0; id[iCount++] = e2;
            id[iCount++] = e1; id[iCount++] = e2; id[iCount++] = e3;

            //front side
            e0 = da[GetFlatIndex(sizeY,sizeZ,x,y,numZ)];
            e1 = da[GetFlatIndex(sizeY,sizeZ,x+1,y,numZ)];
            e2 = da[GetFlatIndex(sizeY,sizeZ,x,y+1,numZ)];
            e3 = da[GetFlatIndex(sizeY,sizeZ,x+1,y+1,numZ)];

            id[iCount++] = e1; id[iCount++] = e0; id[iCount++] = e2;
            id[iCount++] = e1; id[iCount++] = e2; id[iCount++] = e3;
        }
    }

    //bottom side & top side
    for (int z = 0; z < numZ; z++) {
        for (int x = 0; x < numX; x++) {
            //bottom side
            e0 = da[GetFlatIndex(sizeY,sizeZ,x,0,z)];
            e1 = da[GetFlatIndex(sizeY,sizeZ,x+1,0,z)];
            e2 = da[GetFlatIndex(sizeY,sizeZ,x,0,z+1)];
            e3 = da[GetFlatIndex(sizeY,sizeZ,x+1,0,z+1)];

            id[iCount++] = e1; id[iCount++] = e0; id[iCount++] = e2;
            id[iCount++] = e1; id[iCount++] = e2; id[iCount++] = e3;

            
            //top side
            e0 = da[GetFlatIndex(sizeY,sizeZ,x,numY,z)];
            e1 = da[GetFlatIndex(sizeY,sizeZ,x+1,numY,z)];
            e2 = da[GetFlatIndex(sizeY,sizeZ,x,numY,z+1)];
            e3 = da[GetFlatIndex(sizeY,sizeZ,x+1,numY,z+1)];

            id[iCount++] = e1; id[iCount++] = e0; id[iCount++] = e2;
            id[iCount++] = e1; id[iCount++] = e2; id[iCount++] = e3;
        }
    }

    delete[] da;

    //
    NxCookingInterface *cooking = NxGetCookingLib(NX_PHYSICS_SDK_VERSION);

    cooking->NxInitCooking();

    MemoryWriteBuffer wb;
    cooking->NxCookClothMesh(clothMeshDesc, wb);
    MemoryReadBuffer rb(wb.data);
    NxClothMesh* clothMesh = scene->getPhysicsSDK().createClothMesh(rb);
    //

    const NxU32 TEAR_MEMORY_FACTOR = 1;
    NxU32 maxVertices = TEAR_MEMORY_FACTOR * clothMeshDesc.numVertices;
    NxU32 maxIndices = 3 * clothMeshDesc.numTriangles;

    NxClothDesc clothdesc;
    clothdesc.globalPose.t = pos;
    clothdesc.thickness = 0.2f;
    clothdesc.clothMesh = clothMesh;
    clothdesc.meshData = *CreateReceiveBuffer(maxVertices, maxIndices);;
    clothdesc.flags |= NX_CLF_VISUALIZATION;
    if(PhysxLogic::IsHardwareAccelerated() == true)
        clothdesc.flags |= NX_CLF_HARDWARE;

    NxCloth *cloth = scene->createCloth(clothdesc);

    return cloth;
}

NxCloth *ClothScene::CreateClothFromMesh(const NxVec3 &pos, NxTriangleMeshShapeDesc *triMeshShapeDesc, NxScene *scene)
{

    NxTriangleMeshDesc triMeshDesc;
    triMeshShapeDesc->meshData->saveToDesc(triMeshDesc);

    NxClothMeshDesc clothMeshDesc;
    clothMeshDesc.numVertices				= triMeshDesc.numVertices;
    clothMeshDesc.numTriangles				= triMeshDesc.numTriangles;
    clothMeshDesc.pointStrideBytes			= sizeof(NxVec3);
    clothMeshDesc.triangleStrideBytes		= 3*sizeof(NxU32);
    clothMeshDesc.vertexMassStrideBytes		= sizeof(NxReal);
    clothMeshDesc.vertexFlagStrideBytes		= sizeof(NxU32);
    clothMeshDesc.points					= triMeshDesc.points;
    clothMeshDesc.triangles					= triMeshDesc.triangles;
    clothMeshDesc.vertexMasses				= 0;
    clothMeshDesc.vertexFlags				= 0;
    clothMeshDesc.flags						= 0;

    //
    NxCookingInterface *cooking = NxGetCookingLib(NX_PHYSICS_SDK_VERSION);

    cooking->NxInitCooking();

    MemoryWriteBuffer wb;
    cooking->NxCookClothMesh(clothMeshDesc, wb);
    MemoryReadBuffer rb(wb.data);
    NxClothMesh* clothMesh = scene->getPhysicsSDK().createClothMesh(rb);
    //

    const NxU32 TEAR_MEMORY_FACTOR = 1;
    NxU32 maxVertices = TEAR_MEMORY_FACTOR * clothMeshDesc.numVertices;
    NxU32 maxIndices = 3 * clothMeshDesc.numTriangles;

    NxClothDesc clothdesc;
    clothdesc.globalPose.t = pos;
    clothdesc.thickness = 0.2f;
    clothdesc.clothMesh = clothMesh;
    clothdesc.meshData = *CreateReceiveBuffer(maxVertices, maxIndices);
    clothdesc.flags |= NX_CLF_VISUALIZATION;
    if(PhysxLogic::IsHardwareAccelerated() == true)
        clothdesc.flags |= NX_CLF_HARDWARE;

    return scene->createCloth(clothdesc);
}

NxSoftBody *ClothScene::CreateSoftBodyBox(const NxVec3 &pos, const NxVec3 &size, NxScene *scene)
{
    int numX = size.x;
    int numY = size.y;
    int numZ = size.z;
    float h = 1.0f;
    int sizeX = numX + 1;
    int sizeY = numY + 1;
    int sizeZ = numZ + 1;

    NxSoftBodyMeshDesc softBodyMeshDesc;

    softBodyMeshDesc.numVertices = sizeX * sizeY * sizeZ;
    softBodyMeshDesc.numTetrahedra = numX * numY * numZ * 5;
    softBodyMeshDesc.vertexStrideBytes = sizeof(NxVec3);
    softBodyMeshDesc.tetrahedronStrideBytes = 4*sizeof(NxU32);
    softBodyMeshDesc.vertexMassStrideBytes = sizeof(NxReal);
    softBodyMeshDesc.vertexFlagStrideBytes = sizeof(NxU32);
    softBodyMeshDesc.vertices = new NxVec3[softBodyMeshDesc.numVertices];//(NxVec3*)malloc(sizeof(NxVec3)*softBodyMeshDesc.numVertices);
    softBodyMeshDesc.tetrahedra = new NxU32[softBodyMeshDesc.numTetrahedra * 4];//(NxU32*)malloc(sizeof(NxU32)*softBodyMeshDesc.numTetrahedra * 4);
    softBodyMeshDesc.vertexMasses = 0;
    softBodyMeshDesc.vertexFlags = 0;
    softBodyMeshDesc.flags = 0;

    int i,j,k;
    NxVec3 offset(h * numX * 0.5f, h * numY * 0.5f, h * numZ * 0.5f);
    NxVec3 *p = (NxVec3*)softBodyMeshDesc.vertices;
    int counter = 0;
    for (i = 0; i <= numX; i++) {
        for (j = 0; j <= numY; j++) {
            for (k = 0; k <= numZ; k++) {
                p[counter].set(h*i, h*j, h*k);
                p[counter] -=offset;
                counter++;
            }
        }
    }

    int e0,e1,e2,e3,e4,e5,e6,e7;
    int iCount = 0;
    NxU32 *id = (NxU32*)softBodyMeshDesc.tetrahedra;
    for (i = 0; i < numX; i++) {
        for (j = 0; j < numY; j++) {
            for (k = 0; k < numZ; k++) {
                // compute the 8 corner vertices of the cube element
                e4 = (i*sizeY + j)*sizeZ + k; e0 = e4+1;
                e5 = ((i+1)*sizeY + j)*sizeZ + k; e1 = e5+1;
                e6 = ((i+1)*sizeY + (j+1))*sizeZ + k; e2 = e6+1;
                e7 = (i*sizeY + (j+1))*sizeZ + k; e3 = e7+1;

                // define 5 tetrahedral constraints per cube element,
                // by adding an index quadruple for each one
                if ((i + j + k) % 2 == 1) {
                    id[iCount++] = e0; id[iCount++] = e1; id[iCount++] = e5; id[iCount++] = e2;
                    id[iCount++] = e5; id[iCount++] = e2; id[iCount++] = e6; id[iCount++] = e7;
                    id[iCount++] = e0; id[iCount++] = e7; id[iCount++] = e3; id[iCount++] = e2;
                    id[iCount++] = e0; id[iCount++] = e5; id[iCount++] = e4; id[iCount++] = e7;
                    id[iCount++] = e0; id[iCount++] = e2; id[iCount++] = e5; id[iCount++] = e7;
                }
                else {
                    id[iCount++] = e1; id[iCount++] = e4; id[iCount++] = e0; id[iCount++] = e3;
                    id[iCount++] = e1; id[iCount++] = e6; id[iCount++] = e5; id[iCount++] = e4;
                    id[iCount++] = e1; id[iCount++] = e3; id[iCount++] = e2; id[iCount++] = e6;
                    id[iCount++] = e4; id[iCount++] = e6; id[iCount++] = e7; id[iCount++] = e3;
                    id[iCount++] = e1; id[iCount++] = e4; id[iCount++] = e3; id[iCount++] = e6;
                }
            }
        }
    }

    //
    NxCookingInterface *cooking = NxGetCookingLib(NX_PHYSICS_SDK_VERSION);

    cooking->NxInitCooking();

    MemoryWriteBuffer wb;
    cooking->NxCookSoftBodyMesh(softBodyMeshDesc, wb);
    MemoryReadBuffer rb(wb.data);
    NxSoftBodyMesh* softBodyMesh = scene->getPhysicsSDK().createSoftBodyMesh(rb);
    //

    NxU32 maxVertices = softBodyMeshDesc.numVertices;
    NxU32 maxIndices = 4 * softBodyMeshDesc.numTetrahedra;

    NxSoftBodyDesc softBodyDesc;
    softBodyDesc.softBodyMesh = softBodyMesh;
    softBodyDesc.meshData = *CreateReceiveBuffer(maxVertices, maxIndices);
    softBodyDesc.globalPose.t = pos;
    softBodyDesc.particleRadius = 0.2f;
    softBodyDesc.volumeStiffness = 1.0f;
    softBodyDesc.stretchingStiffness = 0.2f;
    softBodyDesc.friction = 1.0f;
    softBodyDesc.solverIterations = 5;
    softBodyDesc.flags |= NX_SBF_VISUALIZATION;
    return scene->createSoftBody(softBodyDesc);
}

NxMeshData *ClothScene::CreateReceiveBuffer(int maxVertices, int maxIndices)
{
    NxU32 *numVertices = new NxU32(0);
    NxU32 *numIndices  = new NxU32(0);

    NxVec3 *vertices = new NxVec3[maxVertices];
    NxVec3 *normals = new NxVec3[maxVertices];
    NxU32 *indices = new NxU32[maxIndices];

    NxMeshData *receiveBuffers = new NxMeshData();

    receiveBuffers->verticesPosBegin         = &(vertices[0].x);
    receiveBuffers->verticesNormalBegin      = &(normals[0].x);
    receiveBuffers->verticesPosByteStride    = sizeof(NxVec3);
    receiveBuffers->verticesNormalByteStride = sizeof(NxVec3);
    receiveBuffers->maxVertices              = maxVertices;
    receiveBuffers->numVerticesPtr           = numVertices;

    receiveBuffers->indicesBegin             = &(indices[0]);
    receiveBuffers->indicesByteStride        = sizeof(NxU32);
    receiveBuffers->maxIndices               = maxIndices;
    receiveBuffers->numIndicesPtr            = numIndices;

    return receiveBuffers;
}

void ClothScene::Update()
{
}

void ClothScene::Draw()
{
    NxCloth **cloths = PhysxLogic::physicsScene->getCloths();
    NxU32 numCloths = PhysxLogic::physicsScene->getNbCloths();

    glDisable(GL_CULL_FACE);
    while(numCloths--)
    {
        NxCloth *cloth = *cloths++;

        NxMeshData mData = cloth->getMeshData();
        NxU32 numI = *mData.numIndicesPtr/3;

        NxU32 *triangles = (NxU32*)mData.indicesBegin;
        NxVec3 *vertices = (NxVec3*)mData.verticesPosBegin;
        NxVec3 *normals = (NxVec3*)mData.verticesNormalBegin;

        glColor3f(0.8f, 0.2f, 0.1f);

        glEnableClientState(GL_VERTEX_ARRAY);
        glEnableClientState(GL_NORMAL_ARRAY);

        glVertexPointer(3, GL_FLOAT, 0, &vertices[0].x);
        glNormalPointer(GL_FLOAT, 0, &normals[0].x);

        glDrawElements(GL_TRIANGLES, numI*3, GL_UNSIGNED_INT, &triangles[0]);

        glDisableClientState(GL_NORMAL_ARRAY);
        glDisableClientState(GL_VERTEX_ARRAY);
    }

    NxSoftBody **softbodies = PhysxLogic::physicsScene->getSoftBodies();
    NxU32 numSoftBodies = PhysxLogic::physicsScene->getNbSoftBodies();

    while(numSoftBodies--)
    {
        NxSoftBody *softbody = *softbodies++;

        NxMeshData mData = softbody->getMeshData();
        NxU32 numTetra = *mData.numIndicesPtr/4;
        NxVec3 *vertices = (NxVec3*)mData.verticesPosBegin;
        NxU32 *idxPtr = (NxU32*)mData.indicesBegin;

        NxVec3 *renderVertices = new NxVec3[12*numTetra];
        NxVec3 *renderNormals = new NxVec3[12*numTetra];

        NxU32 renderVertIdx = 0; 
        NxU32 renderNormalIdx = 0;
        NxReal scale = 0.99f;

        for(NxU32 i = 0; i < numTetra; i++)
        {
            NxVec3 p0(vertices[*idxPtr++]);
            NxVec3 p1(vertices[*idxPtr++]);
            NxVec3 p2(vertices[*idxPtr++]);
            NxVec3 p3(vertices[*idxPtr++]);
            NxVec3 normal, center, v[4];

            center = p0;  v[0] = p0;
            center += p1; v[1] = p1;
            center += p2; v[2] = p2;
            center += p3; v[3] = p3;
            center *= 0.25f;
            v[0] = center + (v[0] - center)*scale;
            v[1] = center + (v[1] - center)*scale;
            v[2] = center + (v[2] - center)*scale;
            v[3] = center + (v[3] - center)*scale;
            NxU8 sides[4][3] = {{2,1,0}, {0,1,3}, {1,2,3}, {2,0,3}};

            for (NxU8 side = 0; side < 4; side ++) {
                NxVec3 &v0 = v[sides[side][0]];
                NxVec3 &v1 = v[sides[side][1]];
                NxVec3 &v2 = v[sides[side][2]];

                normal.cross(v1-v0, v2-v0);
                normal.normalize();

                renderVertices[renderVertIdx++] = v0;
                renderVertices[renderVertIdx++] = v1;
                renderVertices[renderVertIdx++] = v2;

                renderNormals[renderNormalIdx++] = normal;
                renderNormals[renderNormalIdx++] = normal;
                renderNormals[renderNormalIdx++] = normal;
            }
        }

        glColor3f(0.2f, 0.8f, 0.4f);
        glEnableClientState(GL_VERTEX_ARRAY);
        glEnableClientState(GL_NORMAL_ARRAY);

        glVertexPointer(3, GL_FLOAT, 0, &renderVertices[0].x);
        glNormalPointer(GL_FLOAT, 0, &renderNormals[0].x);
        glDrawArrays(GL_TRIANGLES, 0, 12*numTetra); 

        glDisableClientState(GL_NORMAL_ARRAY);
        glDisableClientState(GL_VERTEX_ARRAY);

        delete[] renderVertices;
        delete[] renderNormals;
    }
    glEnable(GL_CULL_FACE);
}

void ClothScene::HandleKeyboardEvent(unsigned char key, int x, int y)
{
    switch(key)
    {
        case 'w': //UP
        {
            NxVec3 dir = PhysxLogic::cam->GetDirection();
            dir.normalize();
            PhysxLogic::cam->MoveBy(dir*1.0);
            break;
        }
        case 's': //DOWN
        {
            NxVec3 dir = PhysxLogic::cam->GetDirection();
            dir.normalize();
            PhysxLogic::cam->MoveBy(-dir*1.0);
            break;
        }
        case 'd': //RIGHT
        {
            NxVec3 dir = PhysxLogic::cam->GetSideNormal();
            dir.normalize();
            PhysxLogic::cam->MoveBy(dir*1.0);
            break;
        }
        case 'a': //LEFT
        {
            NxVec3 dir = PhysxLogic::cam->GetSideNormal();
            dir.normalize();
            PhysxLogic::cam->MoveBy(-dir*1.0);
            break;
        }
        case 'x':
        {
            if(testSoftBody)
            {
                testSoftBody->wakeUp();
                testSoftBody->setFlags(testSoftBody->getFlags() ^ NX_SBF_VOLUME_CONSERVATION);
            }
            if(testcloth)
            {
                testcloth->wakeUp();
                testcloth->setFlags(testcloth->getFlags() ^ NX_CLF_PRESSURE);
            }
            break;
        }
        case '+':
        {
            if(testSoftBody)
            {
                testSoftBody->setVolumeStiffness(testSoftBody->getVolumeStiffness()+0.01f);
                std::cout << "softBodyVolStiffness:" << testSoftBody->getVolumeStiffness() << std::endl;
            }
            if(testcloth)
            {
                testcloth->setPressure(testcloth->getPressure()+0.01f);
                std::cout << "clothPressure:" << testcloth->getPressure() << std::endl;
            }
            break;
        }
        case '-':
        {
            if(testSoftBody)
            {
                testSoftBody->setVolumeStiffness(testSoftBody->getVolumeStiffness()-0.01f);
                std::cout << "softBodyVolStiffness:" << testSoftBody->getVolumeStiffness() << std::endl;
            }
            if(testcloth)
            {
                testcloth->setPressure(testcloth->getPressure()-0.01f);
                std::cout << "clothPressure:" << testcloth->getPressure() << std::endl;
            }
            break;
        }
    }
}

void ClothScene::HandleKeyboardSpecialEvent(int key, int x, int y)
{
    switch(key)
    {
        case GLUT_KEY_F2:
        {
            if(PhysxLogic::IsDebugRenderingEnabled())
            {
                PhysxLogic::SetDebugRenderingEnabled(false);
                PhysxLogic::debugRenderer->DeactivateDebugVisualization(NX_VISUALIZE_CLOTH_MESH);
                PhysxLogic::debugRenderer->DeactivateDebugVisualization(NX_VISUALIZE_CLOTH_COLLISIONS);
                PhysxLogic::debugRenderer->DeactivateDebugVisualization(NX_VISUALIZE_COLLISION_SHAPES);
                PhysxLogic::debugRenderer->DeactivateDebugVisualization(NX_VISUALIZE_SOFTBODY_MESH);	
            }
            else
            {
                PhysxLogic::SetDebugRenderingEnabled(true);
                PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_CLOTH_MESH);
                PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_CLOTH_COLLISIONS);
                PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_COLLISION_SHAPES);
                PhysxLogic::debugRenderer->ActivateDebugVisualization(NX_VISUALIZE_SOFTBODY_MESH);	
            }
            break;
        }
    }
}

void ClothScene::HandleMouseClickEvent(int button, int state, int x, int y)
{

}

void ClothScene::HandleMousePressedMovementEvent(int x, int y, int dx, int dy)
{

}
